#pragma once

#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/* Includes ----------------------------------------------------------------- */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct {
  float pit;
  float yaw;
  float rol;
} Euler_t;

typedef struct {
  float dt; /* 采样时间 */
  /* 测量噪声，R增大，动态响应变慢，收敛稳定性变好 */
  float R_angle; /* 加速度计的噪声协方差 */
  /*过程噪声，Q增大，动态响应变快，收敛稳定性变坏 */
  float Q_gyro;  /* 陀螺仪漂移噪声的协方差 */
  float Q_angle; /* 陀螺仪噪声的协方差 */
  float Q_bias;
  float out;
  float P[2][2];
  float H; /* H矩阵 */
} Filter_Kalman_t;

typedef struct {
  float k;
  float out;
} Filter_LowPass_t;

typedef struct {
  float k;
  float freq;
  float out;
} ComplyFilter_t;

typedef struct {
  struct {
    float x;
    float y;
    float z;
  } acc;
  struct {
    float x;
    float y;
    float z;
  } gyro;
} RawFloat_t;
/* Exported functions prototypes -------------------------------------------- */

int8_t Filter_KalmanInit(Filter_Kalman_t* p, float T_Q);
float Filter_KalmanApply(float acc, float gyro, Filter_Kalman_t* p);

int8_t Filter_LowPassInit(Filter_LowPass_t* filter, float k);
float Filter_LowPassApply(Filter_LowPass_t* filter, float new);

int8_t Filter_ComplyInit(ComplyFilter_t* filter, float k, uint32_t freq);
float Filte_ComplyApply(ComplyFilter_t* filter, float angle, float gyro);
#ifdef __cplusplus
}
#endif
